#ifndef __PID_H__
#define __PID_H__

#include "main.h"

#define ABS(x)		((x>0)? (x): (-x)) 

/**
 * @brief 	检测pid调节是否让期望值达到稳态
 * @param 	pid		pid参数结构体指针
 * @retval  uint32_t	@为 1 (稳定) 或者 0(未达稳定)
 */
#define PID_STABLE_CHECK(pid) \
    ((ABS((pid)->err[NOW]) < (pid)->deadband && \
      ABS((pid)->err[LAST]) < (pid)->deadband && \
      ABS((pid)->err[LLAST]) < (pid)->deadband) ? 1 : 0)
	  
/**
 * @brief 	清除累计的积分
 * @param 	pid		pid参数结构体指针
 */
#define PID_T_CLEAR_INTERGRAL(pid) ( ((pid) -> integral) = 0.0f )

enum{
    LLAST	= 0,
    LAST 	= 1,
    NOW 	= 2,
    POSITION_PID = 4,
    DELTA_PID = 5,
	D_FIRST_EN = 6,
	D_FIRST_DISEN = 7,
};

typedef struct __pid_t
{
    float p;	//p
    float i;	//i
    float d;	//d
	float f;	//f	前馈系数
    
    float set[3];					//目标值,包含NOW， LAST， LLAST上上次
    float get[3];					//测量值
    float err[3];					//误差
	
    float integral;				//积分项
	
    float pout;						//p输出
    float iout;						//i输出
    float dout;						//d输出
    
    float pos_out;					//本次位置式输出
    float last_pos_out;				//上次输出
    float delta_u;					//本次增量值
    float delta_out;				//本次增量式输出 = last_delta_out + delta_u
    float last_delta_out;			//上次增量式输出
    
	#if 0 
	float max_err; 					//最大处理误差（暂不使用）
	#endif
	
	float deadband;					//死区值
    uint32_t pid_mode;				//pid模式
	uint32_t d_first_mode;			//微分先行模式
    uint32_t MaxOutput;				//输出限幅
    uint32_t IntegralLimit;			//积分限幅
    
}pid_t;

void PID_struct_init(
    pid_t* 		pid,
    uint32_t 	mode,
	uint32_t 	d_first_mode,
    uint32_t 	maxout,
    uint32_t 	intergral_limit,
    float	deadband,
    float 	kp, 
    float 	ki, 
    float 	kd,
	float   kf	);
void abs_limit(float *a, float ABS_MAX);
float pid_calc(pid_t* pid, float get_v, float target_v);
float chassis_pid_calc(pid_t* pid, float get_v, float target_v);
float angle_rel_pid_calc(pid_t* pid, float get_v, float target_v);
#endif

